﻿using System;
using System.Linq;
using System.Collections.Generic;
using System.Text;
using MLRobotic.Tower.Detection;
using MLRobotic.Tower.Common;
using DirectShowNETCF;
using MLRobotic.Tower.Messaging;
using System.Drawing;
using System.Diagnostics;
using System.Drawing.Imaging;
using System.Threading;
using DirectShowNETCF.Camera.AMCamera;
using System.Runtime.InteropServices;

namespace MLRobotic.Tower
{
    public class ControlTower : IDisposable
    {
        #region enum
        private enum State
        {
            CalibratingCandles,
            CalibratingBeacons,
            Ready,
            Started,
            Stopped
        }
        #endregion

        #region Privates
        private Log _log = new Log("ControlTower");
        private Stopwatch _watch = new Stopwatch();

        Config _config;

        private AMCamera _camera = null;
        IntPtr _frameBuffer;

        private ImageProcessor _processor;
        private AutoResetEvent _processingTerminated = new AutoResetEvent(true);

        private State _state = State.CalibratingCandles;

        private ushort _lastCandlesSent;
        private int _beaconId = 0;
        private int _lastReceivedBeaconId = 0;
        private Point _lastReceivedBeaconsPolarCoord1;
        private Point _lastReceivedBeaconsPolarCoord2;
        #endregion

        public bool DisplayPixels { get; set; }

        public COM ComPhone;
        public COM ComRobot1;
        public COM ComRobot2;

        public ControlTower(Config config)
        {
            _config = config;

            _camera = new AMCamera();
            _frameBuffer = Marshal.AllocHGlobal(_config.Width * _config.Height * BitmapHelper.GetBytesPerPixel(_config.PixelFormat));

            _processor = new ImageProcessor(_config);

            Messenger.Instance.MatchStarted = () => this.StartMatch();
            Messenger.Instance.FindBeacons = id => this.OnFindBeacons(id);
            Messenger.Instance.ReceivedBeacons = (id, polar) => this.OnReceivedBeacons(id, polar);
        }

        public string Init()
        {
            _log.Debug("Init");

            try
            {
                var r = _camera.init(true);

                if (r != DirectShowNETCF.Camera.AMCamera.AMResult.OK)
                {
                    _log.Error("Cannot Init camera");
                    return "Cannot Init camera";
                }

                if (!_camera.run(IntPtr.Zero))
                {
                    _log.Error("Cannot start camera");
                    return "Cannot start camera";
                }
            }
            catch (Exception ex)
            {
                _log.Error("Starting camera failed", ex);
                return "Starting camera failed : " + ex.Message;
            }

            return null;
        }

        public void AutoFocus(bool active)
        {
            _log.Info("AutoFocus : " + active.ToString());

            if (active)
                _camera.autoFocusOn();
            else
                _camera.autoFocusOff();
        }

        public void CalibrateCandles()
        {
            _log.Info("CalibrateCandles");
            _state = State.CalibratingCandles;
        }

        public void CalibrateBeacons()
        {
            _log.Info("CalibrateBeacons");
            _state = State.CalibratingBeacons;
        }

        public void ReadyForMatch()
        {
            _log.Info("ReadyForMatch");
            _state = State.Ready;
        }

        public void StartMatch()
        {
            _log.Info("StartMatch");
            _state = State.Started;
        }

        public void StopMatch()
        {
            _log.Info("StopMatch");
            _state = State.Stopped;
        }

        public void MainLoop()
        {
            //_log.Debug("MainLoop");

            try
            {
                if (_config.IsTouchPro2())
                    ProcessFrame();

                ProcessCom();
            }
            catch (Exception ex)
            {
                _log.Error("Error in main loop", ex);
            }
        }

        private void ProcessFrame()
        {
            //_log.DebugStartWatch("ProcessFrame");

            _camera.getRgb565(_frameBuffer);
            _processor.SetImage(_frameBuffer);

            bool[] candles = new bool[10];
            Point beacon1 = new Point(), beacon2 = new Point();

            bool send = _state == State.Ready;
            bool displayPixel = (_state == State.CalibratingBeacons || _state == State.CalibratingCandles) && DisplayPixels;

            switch (_state)
            {
                case State.CalibratingCandles:
                case State.Ready:
                    candles = _processor.FindCandlesBySplittingArea(displayPixel);
                    candles = _processor.AverageCandlesOnHistory(candles);
                    _log.Info("Candles = " + candles.ToBoolString());

                    if (ComRobot1 != null && send)
                    {
                        ushort message = candles.ToUShort();
                        if (_lastCandlesSent != message)
                        {
                            _log.Debug("Sending candles : " + message.ToString());
                            ComRobot1.Send(message, 0, PacketType.RetourBougie);
                            _lastCandlesSent = message;
                        }
                    }
                    break;

                case State.CalibratingBeacons:
                case State.Started:
                    send = false; // TODO Activate Beacons

                    DateTime? dueDate = null;
                    if (ComPhone != null)
                    {
                        _beaconId++;
                        _log.Debug(String.Format("Sending DemandePositionAdversaire (id: {0})", _beaconId));
                        ComPhone.Send((short)_beaconId, PacketType.DemandePositionAdversaire);
                        dueDate = DateTime.Now.Add(TimeSpan.FromMilliseconds(1000));
                    }

                    IList<Point> beaconsRelativePolar = _processor.FindBeaconsByVectors(displayPixel).ToList();
                    GetBeaconCoordinates(beaconsRelativePolar, dueDate, out beacon1, out beacon2);

                    if (ComRobot1 != null && send)
                    {
                        if (beacon1.X != 0 || beacon1.Y != 0)
                        {
                            _log.Debug(String.Format("Sending beacons ({0}, {1})", beacon1.X, beacon1.Y));
                            ComRobot1.Send((short)beacon1.X, (short)beacon1.Y, PacketType.RetourPositionAdversaire);
                        }

                        if (beacon2.X != 0 || beacon2.Y != 0)
                        {
                            _log.Debug(String.Format("Sending beacons ({0}, {1})", beacon2.X, beacon2.Y));
                            ComRobot1.Send((short)beacon2.X, (short)beacon2.Y, PacketType.RetourPositionAdversaire);
                        }
                    }
                    break;
            }
            
            RaiseFrameEvent(_processor.LastResult, candles, beacon1, beacon2);

            //_log.DebugElapsed("ProcessFrame");
        }

        private void GetBeaconCoordinates(IList<Point> beaconsRelativePolar, DateTime? dueDate, out Point beacon1, out Point beacon2)
        {
            beacon1 = new Point();
            beacon2 = new Point();

            if (!dueDate.HasValue)
            {
                beacon1 = beaconsRelativePolar.Count > 0 ? _processor.GetBeaconCoordinates(beaconsRelativePolar[0]) : new Point();
                beacon2 = beaconsRelativePolar.Count > 1 ? _processor.GetBeaconCoordinates(beaconsRelativePolar[1]) : new Point();
            }
        }

        private void ProcessCom()
        {
            //_log.DebugStartWatch("ProcessCom");

            COM com = ComRobot1;
            if (com != null)
                while (com.Process()) { }

            com = ComRobot2;
            if (com != null)
                while (com.Process()) { }

            com = ComPhone;
            if (com != null)
                while (com.Process()) { }

            //_log.DebugElapsed("ProcessCom");
        }

        /// <summary>
        /// Called when FindBeacons message is received. Used on slave phone.
        /// </summary>
        /// <param name="id"></param>
        private void OnFindBeacons(int id)
        {
            _log.DebugStartWatch("OnFindBeacons");

            var beaconsRelativePolar = _processor.FindBeaconsByVectors(false).ToList();
            
            if (ComPhone != null)
            {
                if (beaconsRelativePolar.Count > 0)
                {
                    _log.Debug(String.Format("Sending polar beacons 1 ({0}, {1})", beaconsRelativePolar[0].X, beaconsRelativePolar[0].Y));
                    ComPhone.Send((short)id, (short)beaconsRelativePolar[0].X, (short)beaconsRelativePolar[0].Y, PacketType.RetourPositionAdversaire);
                }
                else
                {
                    _log.Debug("Sending polar beacons 1 NOT FOUND(0, 0)");
                    ComPhone.Send((short)id, 0, 0, PacketType.RetourPositionAdversaire);
                }

                if (beaconsRelativePolar.Count > 1)
                {
                    _log.Debug(String.Format("Sending polar beacons 2 ({0}, {1})", beaconsRelativePolar[1].X, beaconsRelativePolar[1].Y));
                    ComPhone.Send((short)id, (short)beaconsRelativePolar[1].X, (short)beaconsRelativePolar[1].Y, PacketType.RetourPositionAdversaire);
                }
                else
                {
                    _log.Debug("Sending polar beacons 2 NOT FOUND(0, 0)");
                    ComPhone.Send((short)id, 0, 0, PacketType.RetourPositionAdversaire);
                }
            }

            _log.DebugElapsed("OnFindBeacons");
        }

        /// <summary>
        /// Called when ReceivedBeacons message is received. Used on master phone.
        /// </summary>
        /// <param name="id"></param>
        /// <param name="alpha"></param>
        /// <param name="dist"></param>
        private void OnReceivedBeacons(int id, Point polar)
        {
            if (id < _beaconId)
                return;

            if (_lastReceivedBeaconId != id)
            {
                _lastReceivedBeaconsPolarCoord1 = polar;
                _lastReceivedBeaconsPolarCoord2 = new Point();
            }
            else
                _lastReceivedBeaconsPolarCoord2 = polar;

            _lastReceivedBeaconId = id;
        }

        public void Dispose()
        {
            _log.Debug("Dispose");

            if (ComRobot1 != null)
                ComRobot1.Dispose();

            if (ComRobot2 != null)
                ComRobot2.Dispose();

            if (ComPhone != null)
                ComPhone.Dispose();

            if (_camera != null)
            {
                _camera.stop();
                _camera.release();
            }

            Marshal.FreeHGlobal(_frameBuffer);
        }

        #region FrameProcessed event
        public event EventHandler<FrameProcessedEventArgs> FrameProcessed;

        public class FrameProcessedEventArgs : EventArgs
        {
            public Bitmap Frame;
            public bool[] Candles;
            public Point Beacon1;
            public Point Beacon2;
        }

        private void RaiseFrameEvent(Bitmap frame, bool[] candles, Point beacon1, Point beacon2)
        {
            var handler = this.FrameProcessed;
            if (handler != null)
            {
                handler(this, new FrameProcessedEventArgs { Frame = frame, Candles = candles, Beacon1 = beacon1, Beacon2 = beacon2 });
            }
        }
        #endregion
    }
}
